# -*- coding:utf-8 -*-
from ctypes import *
import time
import numpy as np

# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
import platform
arch = platform.machine()
sys.path.append(join(dirname(__file__), 'src/lysdemo_'+arch))
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *

# ###########################################################################


if __name__ == "__main__":
    
    # >>>>>初始化CAN卡部分>>>
    # 设置can or canfd接口
    lcanlib = LCAN()
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)

    # open device
    ret = lcanlib.LCAN_OpenDevice(LCAN_USBCANFDMini, devIndex)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFDMini Open Device failed!")
        exit(0)
    # open success
    print("LYS USBCANFDMini Open Device Success!")


    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)

    # 初始化canfd
    ret = lcanlib.LCAN_InitCANFD(LCAN_USBCANFDMini, devIndex, canIndex, canfd_init_cfg)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANmini Init Failed!")
    else:
        print("LYS USBCANmini Init Success!")
    

    # 清除can通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    print('sleeping')
    time.sleep(0.1)
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    motor_controllers = MotorController(motor_num)

    # 清除错误
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_clear_err, motor_num)
    print('sleeping')
    time.sleep(0.1)

    # 上使能
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_enable, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("Enable Failed!")
    else:
        print("Enable Success!")
    

    # 在线更新标志位
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_obj_init, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("motor init Failed!")
    else:
        print("motor init Success!")



    # 设置p模式
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_p_mode, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("Set p mode failed!")
    else:
        print("Set p mode success!")

    

    
    
    
    
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # 控制
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    for ID in motor_ids:
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_obj_get[ID-1], 1)
        time.sleep(0.1)
    q_real = receive_p_state(lcanlib, devIndex, canIndex)
    bbbbb = q_real
    print(f"q_real = {q_real}")


    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    #Close Device
    lcanlib.LCAN_CloseDevice(LCAN_USBCANFDMini, devIndex)
    print("LYS USBCANmini Close!")